/*! \file	depth.hpp
 *  \brief	Declarations for the DEPTH node.
*/

#ifndef _THERMALVIS_DEPTH_H_
#define _THERMALVIS_DEPTH_H_

#include "time.h"

#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>

#include <boost/thread/thread.hpp>

#include <ros/ros.h>

// #include <image_geometry/image_geometry.h>

// #include "/opt/ros/electric/stacks/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h"
#include <image_geometry/stereo_camera_model.h>

namespace enc = sensor_msgs::image_encodings;

//#include "../../shared-src/super.h"
#include "ros_resources.hpp"
#include "general_resources.hpp"
#include "opencv_resources.hpp"
#include "camera.hpp"
#include "tools.hpp"
#include "improc.hpp"
#include "stereo.hpp"
// #include "../../shared-src/video.h"
// #include "../../shared-src/reconstruction.h"

#include <dynamic_reconfigure/server.h>
#include "depthConfig.h"

#define MAXIMUM_FRAMES_TO_STORE 100

const char __PROGRAM__[] = "DEPTH";

typedef dynamic_reconfigure::Server < thermalvis::depthConfig > Server;

using namespace std;
using namespace cv;

/// \brief		Stores configuration information for the stereo routine
struct stereoData {
	string video_stream;
	cameraParameters cameraData1, cameraData2;
	
	double alpha;
	bool autoAlpha;
	
	string read_addr;
	string extrinsics;
	
	bool debugMode;
	double maxTimeDiff;
	
	// May also need some extrinsic data, extracted from the streams...
	
	bool obtainStartingData(ros::NodeHandle& nh);   
	
};

/// \brief		Manages the stereo procedure
class stereoDepthNode {
private:

	ros::Timer timer;
	
	bool alphaChanged;
	
	//sensor_msgs::Image disp_image;
	stereo_msgs::DisparityImage disp_object;
	sensor_msgs::PointCloud2 colouredCloud;
	
	image_geometry::StereoCameraModel stereo_model;
	
	sensor_msgs::CameraInfo cam_info_1, cam_info_2;
	
	Mat t;
	Mat R0, R1;
	Mat P0, P1;
	Mat F, Q1, Q2;
	
	unsigned int checkIndex_1, checkIndex_2;
	// unsigned int duplicateFlags_1[MAXIMUM_FRAMES_TO_STORE], duplicateFlags_2[MAXIMUM_FRAMES_TO_STORE];
	ros::Time time_buffer_1[MAXIMUM_FRAMES_TO_STORE], time_buffer_2[MAXIMUM_FRAMES_TO_STORE];
	vector<unsigned int> validPairs[2];
	unsigned int pairsProcessed;
	
	ros::Publisher disparityPublisher;

	stereoData configData;
	
	sensor_msgs::CameraInfo debug_camera_info;
	
	char cam_pub_name_1[256], cam_pub_name_2[256];
	sensor_msgs::Image msg_1, msg_2;
	sensor_msgs::CameraInfo camera_info_1, camera_info_2;
	image_transport::CameraPublisher depth_pub, cam_pub_1, cam_pub_2;
	
	image_transport::CameraSubscriber camera_sub_1, camera_sub_2;
	
	Mat disp16;
	
	cv_bridge::CvImagePtr cv_ptr_1;
	cv_bridge::CvImagePtr cv_ptr_2;
	
	Mat rawImageBuffer_1[MAXIMUM_FRAMES_TO_STORE], rawImageBuffer_2[MAXIMUM_FRAMES_TO_STORE];
	Mat newImage, grayImage, realImage, mappedImage, lastImage_1, lastImage_2, olderImage_1, olderImage_2, normalizedMat;

	Mat map11, map12, map21, map22;
	
	Rect roi1, roi2;

	struct timeval cycle_timer;
	double elapsedTime;

	unsigned int frameCount, frameCount_1, frameCount_2;

	char depth_pub_name[256];
	ros::NodeHandle private_node_handle;
	
	string nodeName;
	
	Mat displayImage, drawImage;
	
	Mat grayImageBuffer_1[MAXIMUM_FRAMES_TO_STORE], grayImageBuffer_2[MAXIMUM_FRAMES_TO_STORE];
	
	Mat blurredImage;
	
	bool infoProcessed_1;
	bool infoProcessed_2;
	
	bool firstImProcessed_1, firstImProcessed_2, firstPairProcessed;
	
	StereoBM bm;
	StereoSGBM sgbm;
	Mat disp, disp8, dispFloat, disparityImage;
	
	int SADWindowSize;
	int numberOfDisparities;
	
	bool firstPair;
	
	boost::shared_mutex _access;
	
	dynamic_reconfigure::Server<thermalvis::depthConfig> server;
	dynamic_reconfigure::Server<thermalvis::depthConfig>::CallbackType f;
	
public:
	stereoDepthNode(ros::NodeHandle& nh, stereoData startupData);
	
	void timed_loop(const ros::TimerEvent& event);
	
	void handle_image_1(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
	void handle_image_2(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
    
    void publishMaps();
	
	void assignDepthCameraInfo();
	
	void updatePairs();
	void updateMaps();
	
	void serverCallback(thermalvis::depthConfig &config, uint32_t level);

};

#endif
